/* 
 * $Id$
 *
 * UMKC Robotics 2009
**/

#include <math.h>
#include "intdefs.h"
#include "2space.h"

#include "err.h"
extern int err;

#ifdef DBG
#include "iopublic.h"
extern const BotIoctl bioctl;
#endif



/* static forwards. */
static inline RectangularPoint rp_get(TwoD*);
static inline void rp_set(TwoD*, RectangularPoint*);
static inline PolarPoint pp_get(TwoD*);
static inline void pp_set(TwoD*, PolarPoint*);

static RectangularPoint rp_from_polar(PolarPoint*);
static PolarPoint pp_from_rect(RectangularPoint*);
static inline Float_T d2r(Float_T);
static inline Float_T r2d(Float_T);

static inline void t_reset(Triangle*);
static inline void t_set_a(Triangle*, Float_T);
static inline void t_set_b(Triangle*, Float_T);
static inline void t_set_c(Triangle*, Float_T);
static inline void t_set_A(Triangle*, Float_T);
static inline Float_T t_A_from_tanInv(Triangle*); 
static inline Float_T t_ccosA(Triangle*);
static inline Float_T t_csinA(Triangle*);
static inline Float_T t_c_from_distance(Triangle*);


/* our position in the known universe.  volatile if this
 * gets manipulated via an ISR() */
#ifdef VOLATILE_UNIVERSE
TwoD volatile Upos = { 0.0, 0.0 };
#else
TwoD Upos = {0.0, 0.0 };
#endif


/* the global 2space controller */ 
const TwoDOps mathops = {
  &rp_get,
  &rp_set,
  &pp_get,
  &pp_set,
  &rp_from_polar,
  &pp_from_rect,
  &d2r,
  &r2d,
  &t_reset,
  &t_set_a,
  &t_set_b,
  &t_set_c,
  &t_set_A,
  &t_A_from_tanInv,
  &t_ccosA, 
  &t_csinA,
  &t_c_from_distance
};




/*-------------------------------------------------------*/
/* implementation below here */
/*-------------------------------------------------------*/

static inline RectangularPoint rp_get(TwoD* u) {
  
  RectangularPoint rp;
  rp.x = u->a;
  rp.y = u->b;
  return( rp ); 

}

static inline void rp_set(TwoD* u, RectangularPoint* rp) {;}

static inline PolarPoint pp_get(TwoD* u) {

  PolarPoint pp;
  pp.r = u->a;
  pp.a = u->b;
  return( pp ); 

}

static inline void pp_set(TwoD* u, PolarPoint* pp) {;}



/* convert from degrees to radians */
static inline Float_T d2r(Float_T deg) {

  return( deg * (PI / 180) );  

}

/* convert from radians to degrees */
static inline Float_T r2d(Float_T rad) {

  return(rad * (180 / PI) );

}

/* convert from polar to rectangular */
static RectangularPoint rp_from_polar(PolarPoint* pp) {

  RectangularPoint rp;
  rp.x =  pp->r * cos(pp->a);   
  rp.y =  pp->r * sin(pp->a);  
  return(rp);

}

/* convert from rectangular to polar.  */
static PolarPoint pp_from_rect(RectangularPoint* rp) {

  PolarPoint pp;
  pp.r = ( sqrt( pow(rp->x,2) + pow(rp->y,2) ) );
  pp.a = atan(rp->y / rp->x);
  return(pp);

}  

/* zero everything out */
static inline void t_reset(Triangle* t) {
  
  t->a = t->b = t->c = t->A = t->B = t->C = 0.0;

}

static inline void t_set_a(Triangle* t, Float_T d) {

  t->a = d;

}

static inline void t_set_b(Triangle* t, Float_T d) {

  t->b = d;

}

static inline void t_set_c(Triangle* t, Float_T d) {

  t->c = d;

}

static inline void t_set_A(Triangle* t, Float_T d) {

  t->A = d;

}

/* length of b (or, x). */
static inline Float_T t_ccosA(Triangle* t) {

  return( t->b = t->c * cos(t->A) );

}
  
/* length of a (or, y). */
static inline Float_T t_csinA(Triangle* t) {

  return( t->a = t->c * sin(t->A) );

}

/* angle of A in radians
 * -pi/2 <= A <= pi/2  */
static inline Float_T t_A_from_tanInv(Triangle* t) {
  
  return( t->A = atan( t->a / t->b ) );
}

static inline Float_T t_c_from_distance(Triangle* t) {

  return(t->c = sqrt( pow(t->a,2) + pow(t->b,2)) );

}
